#!/bin/bash

# Parsing arguments
OPTIONS=$(getopt -o hsc --long help,single,continue -- "$@")
if [ $? -ne 0 ]; then
    echo "Invalid options provided"
    exit 1
fi
eval set -- "$OPTIONS"

usage() {
    echo "Usage: $0 [options]"
    echo "Options:"
    echo "  -h, --help      Show this help message"
    echo "  -s, --single    Run only one test case using current config file"
    echo "  -c, --continue  Continue running tests even if one fails"
    exit 0
}

RUN_SINGLE=false
CONTINUE_ON_FAILURE=false
while true; do
    case "$1" in
    -h | --help)
        usage
        ;;
    -s | --single)
        RUN_SINGLE=true
        shift
        ;;
    -c | --continue)
        CONTINUE_ON_FAILURE=true
        shift
        ;;
    --)
        shift
        break
        ;;
    *)
        break
        ;;
    esac
done

# Setup
rm -rf build/agnocast_e2e_test install/e2e_test
source /opt/ros/humble/setup.bash
colcon build --symlink-install --packages-select agnocast_e2e_test --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.bash

# Run test
CONFIG_FILE="src/agnocast_e2e_test/test/config_test_1to1_with_ros2sub.yaml"
show_config() {
    echo "----------------------------------------------" | sudo tee /dev/kmsg
    cat $CONFIG_FILE | sudo tee /dev/kmsg
    echo "----------------------------------------------" | sudo tee /dev/kmsg
}

FAILURE_COUNT=0
if [ "$RUN_SINGLE" = true ]; then
    show_config
    launch_test src/agnocast_e2e_test/test/test_1to1_with_ros2sub.py
    if [ $? -ne 0 ]; then
        echo "Test failed" | sudo tee /dev/kmsg
        exit 1
    fi
else
    LAUNCH_PUB_BEFORE_SUB=(true false)
    PUB_QOS_DEPTH=(1 10)
    PUB_TRANSIENT_LOCAL=(true false)
    SUB_QOS_DEPTH=(1 10)
    SUB_TRANSIENT_LOCAL=(true false)
    USE_TAKE_SUB=(true false)

    COUNT=0
    for launch_pub_before_sub in ${LAUNCH_PUB_BEFORE_SUB[@]}; do
        for pub_qos_depth in ${PUB_QOS_DEPTH[@]}; do
            for pub_transient_local in ${PUB_TRANSIENT_LOCAL[@]}; do
                for sub_qos_depth in ${SUB_QOS_DEPTH[@]}; do
                    for sub_transient_local in ${SUB_TRANSIENT_LOCAL[@]}; do
                        for use_take_sub in ${USE_TAKE_SUB[@]}; do
                            COUNT=$((COUNT + 1))
                            sed -i "s/launch_pub_before_sub:.*/launch_pub_before_sub: $launch_pub_before_sub/g" $CONFIG_FILE
                            sed -i "s/pub_qos_depth:.*/pub_qos_depth: $pub_qos_depth/g" $CONFIG_FILE
                            sed -i "s/pub_transient_local:.*/pub_transient_local: $pub_transient_local/g" $CONFIG_FILE
                            sed -i "s/sub_qos_depth:.*/sub_qos_depth: $sub_qos_depth/g" $CONFIG_FILE
                            sed -i "s/sub_transient_local:.*/sub_transient_local: $sub_transient_local/g" $CONFIG_FILE
                            sed -i "s/use_take_sub:.*/use_take_sub: $use_take_sub/g" $CONFIG_FILE
                            echo "====================== $COUNT / 64 ======================" | sudo tee /dev/kmsg
                            show_config
                            launch_test src/agnocast_e2e_test/test/test_1to1_with_ros2sub.py

                            if [ $? -ne 0 ]; then
                                echo "Test failed." | sudo tee /dev/kmsg
                                if [ "$CONTINUE_ON_FAILURE" = true ]; then
                                    FAILURE_COUNT=$((FAILURE_COUNT + 1))
                                else
                                    exit 1
                                fi
                            fi
                        done
                    done
                done
            done
        done
    done
fi

if [ "$FAILURE_COUNT" -gt 0 ]; then
    echo "$FAILURE_COUNT / $COUNT tests failed" | sudo tee /dev/kmsg
    exit 1
else
    echo "All tests passed!!" | sudo tee /dev/kmsg
fi

# Reset config file
git checkout -- "$CONFIG_FILE"
